#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from sensor_msgs.msg import JointState
from trajectory_msgs.msg import JointTrajectory
import time
import serial 
from std_msgs.msg import Header
# from serial_move import Ser
from moveit_msgs.msg import RobotTrajectory
from math import sin, cos, pi

import math
import struct
import os


class Ser:
    '''
    串口通信的工具类，用于与STM32进行数据交互
    通过串口发送数据到STM32
    参数：
        port: 串口设备路径，默认为'/dev/ttyS1'
        baudrate: 波特率，默认为115200
    内置函数：
        send_to_stm32: 发送数据到STM32并接收响应    
    用法：
        ser = Ser()
        angles = ...  # 六个关节的角度
        ser.send_to_stm32(angles)
    '''
    def __init__(self, baudrate=115200, port='/dev/ttyS1'):
        print("List of enabled UART:")
        os.system('ls /dev/tty[a-zA-Z]*')
        self.ser = serial.Serial(port, baudrate, timeout=1)
        if self.ser.isOpen():
            print(self.ser.name, 'Open Successed!')
        else:
            print('Open Failed!')

    def send_to_stm32(self, angles: list) -> None:
        '''
        将数据发送给stm32
        '''
        def get_crc8(data: bytes) -> bytes:
            """
            计算CRC8校验值，多项式0x8C
            :param data: 需要校验的字节数据
            :return: CRC8校验值（0~255）
            """
            crc = 0
            for byte in data:
                crc ^= byte
                for _ in range(8):
                    if crc & 0x01:
                        crc = (crc >> 1) ^ 0x8C
                    else:
                        crc >>= 1
                    crc &= 0xFF  # 保证crc始终为8位
            crc = bytes([crc])  # 转换为字节类型
            return crc

        if len(angles) != 6:
            raise ValueError("角度列表必须仅含6个元素")
        
        # 数据打包部分
        begin = b'\x55\xaa'
        end = b'\x0d\x0a'  # 结束符
        length = bytes([13])
        # angles = [int(math.pi / 4 * 1000)] * 6  # 六个关节的角度
        angles = [int(angle * 1000) for angle in angles]  # 将角度转换为弧度制并乘以1000
        print(angles)
        flag = b'\x00'   # 标志位
        # 六个关节的角度(弧度制) *1000, short类型

        # 将角度转换为short类型的字节数据
        angles_data = struct.pack('<hhhhhh', *angles)
        # 计算CRC8校验值
        crc = get_crc8(begin + length + angles_data + flag)
        # 最终发送的数据(bytes类型)
        out_data = begin + length + angles_data + flag + crc + end
        print(out_data) 

        # 串口发送部分
        # ser = self.ser

        len_out_data = self.ser.write(out_data)
        print('输出字节长度:', len_out_data)


        # # 下面是接收数据
        # input_data = ser.read(19)
        # print('input_data:', input_data)

        # begin_in = input_data[0:2]
        # if begin_in == b'\x55\xaa':
        #     print('begin_in: CORRECT')

        # angles_in = input_data[3:15]
        # angles_in = struct.unpack('<hhhhhh', angles_in)
        # print('angles_in:', angles_in)

        # end_in = input_data[17:]
        # if end_in == b'\x0d\x0a':
        #     print('end_in: CORRECT')


        
        self.ser.write(out_data)
        time.sleep(0.01)

        # self.ser.close()

class FeedBackJoint(Node):
    def __init__(self,name):
        super().__init__(name)
        self.ser = Ser(baudrate=115200)  # 替换为实际的串口设备路径
        self.sub_joint=self.create_subscription(RobotTrajectory,"plan_goal_trajectory",self.goal_joint_callback,10)
        self.pub_joint=self.create_publisher(JointState,"/joint_states",10)
        self.get_logger().info("ing....")
        self.last_traject=RobotTrajectory()
        
        self.jointstate=JointState()
        
        self.joint_names = ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6']
        # 初始化關節位置
        self.positions = [0.0,0.0, 0.0, 0.0,0.0, 0.0]
        msg = JointState()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.name = self.joint_names
        msg.header.frame_id = "base_link"


        msg.position = self.positions
        self.pub_joint.publish(msg)
        
        

        
    def goal_joint_callback(self, msg: RobotTrajectory):

        # 直接发全部的轨迹和时间
        # 轨迹里面包含很多个位置，每个位置对应一组关节角度
        if(self.last_traject == msg):
            self.get_logger().info("The same trajectory, skip")
            return
        else:
            self.last_traject = msg
        trajectory=msg.joint_trajectory
        
        # for i, point in enumerate(trajectory.points):
        #     positions = point.positions 
        #     time_from_start = point.time_from_start.sec + point.time_from_start.nanosec * 1e-9
        #     self.get_logger().info(f"Point {i}: positions={positions}, time_from_start={time_from_start:.3f}s")
            
        #     # 更新关节状态            
        #     self.jointstate.header.stamp = self.get_clock().now().to_msg()
        #     self.jointstate.header.frame_id = "base_link"
        #     self.jointstate.name = trajectory.joint_names
        #     self.jointstate.position = positions
        #     self.pub_joint.publish(self.jointstate)

        #     self.ser.send_to_stm32(positions)  # 发送到STM32
        #     # time.sleep(0.001)
        #         #     # 更新关节状态            

        point= trajectory.points[-1]
        positions = point.positions
        time_from_start = point.time_from_start.sec + point.time_from_start.nanosec * 1e-9
        self.get_logger().info(f"Point: positions={positions}, time_from_start={time_from_start:.3f}s")
        self.ser.send_to_stm32(positions)
        self.jointstate.header.stamp = self.get_clock().now().to_msg()
        self.jointstate.header.frame_id = "base_link"
        self.jointstate.name = trajectory.joint_names
        self.jointstate.position = positions
        self.pub_joint.publish(self.jointstate)


def main(args=None):
    rclpy.init(args=args)
    node = FeedBackJoint('hardware_move_node')
    
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()    